/*==============================================================================
 *
 * Chris Rigg and Sam Strom
 *
 * ECEN 5033
 *
 * Spring 2008
 *
 * Much of the code in robot_com_interface.[ch] was adapted from original code
 * provided by the following source:
 *
 * - http://www.captain.at/howto-simple-serial-port-test-example.php
 *
 * - The pthread initialization code was adapted from pthread.c provided
 *   for Lab1 of ECEN5033
 *
 * - Linux Socket code reference :
 *   http://www.cs.utah.edu/~swalton/listings/sockets/programs/
 *
 *
 */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <termios.h>
#include <sched.h>
#include <pthread.h>
#include <sys/socket.h>
#include <resolv.h>
#include <arpa/inet.h>
#include <semaphore.h>
#include <signal.h>

#include "robot_comm_interface.h"

static struct termios options;
static const char *devicenm = "/dev/ttyS0";
static int fd;
static unsigned char cmd[3];

#define PULSE_WIDTH_IN_MSEC (2)

// Thread attributes
static pthread_t comm_thread;
static pthread_attr_t comm_thread_attr;
static int rt_max_prio, rt_min_prio;
static struct sched_param comm_thread_sched_param;
static sem_t start_thread;

// Client/Server interface attributes
static int sockserv;

typedef struct {
  RCI_Joint_Specifier joint;
  unsigned char pos;
} MoveRequest;

static sem_t move_sem;

static int stop_search;

typedef enum {
  RCI_STATE_IDLE = 0,
  RCI_STATE_SEARCH,
  RCI_STATE_GRAB,
  RCI_STATE_DUNK,
  RCI_STATE_MAX  
} RCIState;

static RCIState rci_state = RCI_STATE_IDLE;

#define RCI_PORT 8272
#define RCI_IP   "127.0.0.1"

typedef struct {
  int cmd;

  union {
#define CMD_KILL_REQUEST 0    

#define CMD_MOVE_REQUEST 1    
    MoveRequest move_request;

#define CMD_START_SEARCH_MODE 2

#define CMD_STOP_SEARCH_MODE 3

#define CMD_GET_BALL_REQUEST 4
    struct {
      int location;
    } GetBallRequest;

#define CMD_DUNK_REQUEST 5
    struct {
      int location;
    } DunkRequest;
  } u;
} RCI_CMD;

#define BUF_SZ 1024

static void send_buf_to_serv (unsigned char *buf, int len)
{
  struct sockaddr_in dest;
  int sockfd;

  if ((sockfd = socket(AF_INET, SOCK_STREAM, 0)) < 0)
  {
    perror("Client Socket");
    return;
  }

  // Initialize server address/port struct
  bzero(&dest, sizeof(dest));
  dest.sin_family = AF_INET;
  if ( (dest.sin_addr.s_addr = inet_addr(RCI_IP)) == 0)
  {
    perror(RCI_IP);
    return;
  }
  
  dest.sin_port = htons(RCI_PORT);

  // Connect to server
  if (connect(sockfd, (struct sockaddr *)&dest, sizeof(dest)) != 0)
  {
    perror("Client Connect");
    return;
  }

  // Send the buffer
  send(sockfd, buf, len, 0);

  fprintf(stderr, "RCI Client: Sent %d bytes to serv\n");

  // Close the socket
  close(sockfd);
}

static void init_comm_sockserv (void)
{
  struct sockaddr_in self;
  int val = 1;
  
  // Create streaming socket
  if ((sockserv = socket(AF_INET, SOCK_STREAM, 0)) < 0)
  {
    perror("Server Socket");
    exit(errno);
  }

  // Set socket options to reuse Addr
  setsockopt(sockserv,
	     SOL_SOCKET,
	     SO_REUSEADDR,
	     &val,
	     sizeof val);

  // Initialize address/port structure
  memset (&self, 0, sizeof(self));
  self.sin_family = AF_INET;
  self.sin_port = htons(RCI_PORT);
  self.sin_addr.s_addr = INADDR_ANY;

  // Assign a port number to the socket
  if (bind(sockserv, (struct sockaddr*)&self, sizeof(self)) != 0)
  {
    perror("Server Socket--bind");
    exit(errno);
  }

  // Make it a "listening socket"
  if (listen(sockserv, 20) != 0)
  {
    perror("Server Socket--listen");
    exit(errno);
  }
}

static void melobot_move (RCI_Joint_Specifier joint, unsigned char pos)
{
  int nbytes_written;

  // This servo controller we are using (SSC04A from Pololu)
  // recognizes the Mini SSC II Mode protocol. In this mode,
  // you send commands in a series of 3 byte writes:
  // 1) The start byte (0xFF)
  // 2) The servo specifier (0x00 - 0xFE --> but in our case
  //    it is 0x00 - 0x05 for 6DOF)
  // 3) The servo position (0x00 - 0xFE)
  //
  //
  cmd[0] = 0xFF;
  nbytes_written = write (fd, &cmd[0], 1);
  usleep (PULSE_WIDTH_IN_MSEC*1000);

  cmd[1] = joint;
  nbytes_written = write (fd, &cmd[1], 1);
  usleep (PULSE_WIDTH_IN_MSEC*1000);

  cmd[2] = pos;
  nbytes_written = write (fd, &cmd[2], 1);
  usleep (PULSE_WIDTH_IN_MSEC*1000);

  sleep(1);

  // We have completed the move, post on semaphore
  sem_post(&move_sem);
}

static void set_initial_position (void)
{
  melobot_move (RCI_BASE, 0);
  melobot_move (RCI_SHOULDER, 150);
  melobot_move (RCI_ELBOW, 100);
  melobot_move (RCI_WRIST, 0);
  melobot_move (RCI_HAND, 0);
  melobot_move (RCI_GRIPPER, 254);
}

static int getbaud(void)
{
  struct termios term_attr;
  int input_speed = -1;
  speed_t baud_rate;

  tcgetattr(fd, &term_attr);

  // Get the input speed
  baud_rate = cfgetispeed(&term_attr);
  switch (baud_rate)
  {
  case B0:
    input_speed = 0;
    break;
  case B50:
    input_speed = 50;
    break;
  case B110:
    input_speed = 110;
    break;
  case B134:
    input_speed = 134;
    break;
  case B150:
    input_speed = 150;
    break;
  case B200:
    input_speed = 200;
    break;
  case B300:
    input_speed = 300;
    break;
  case B600:
    input_speed = 600;
    break;
  case B1200:
    input_speed = 1200;
    break;
  case B1800:
    input_speed = 1800;
    break;
  case B2400:
    input_speed = 2400;
    break;
  case B4800:
    input_speed = 4800;
    break;
  case B9600:
    input_speed = 9600;
    break;
  case B19200:
    input_speed = 19200;
    break;
  case B38400:
    input_speed = 38400;
    break;
  }
  return input_speed;
}

static void pan_camera (void)
{
  int pos;

  for (pos = 0; pos < 254; pos+=20)
  {
    if (stop_search) {
      return;
    }

    melobot_move (RCI_BASE, pos);
  }

  /* Pan back to start */
  melobot_move (RCI_BASE, 0);
}

static void handle_search_instruction (void)
{
  int elbow_pos = 100;

  /* Go to starting position */
  fprintf(stderr,
	  "RCI Serv: Setting initial pos for "
	  "search mode\n");
  set_initial_position ();

  fprintf(stderr,
	  "RCI Serv: Melobot searching for "
	  "basketball...\n");

  while (1)
  {
    if (stop_search) {
      break;
    }

    if (elbow_pos > 101)
    {
      elbow_pos = 40;
    }
    else
    {
      elbow_pos += 20;
    }

    melobot_move (RCI_ELBOW, elbow_pos);

    pan_camera();
  }

  stop_search = 0;
}

static void handle_grab_instruction (void)
{
  fprintf(stderr, "RCI Serv: Go GET it Melobot...\n");

  melobot_move(RCI_GRIPPER,254);
  melobot_move(RCI_BASE,254);
  melobot_move(RCI_SHOULDER,50);
  melobot_move(RCI_ELBOW,60);
  melobot_move(RCI_WRIST,0);
  melobot_move(RCI_HAND,0);
  melobot_move(RCI_BASE,100);
  melobot_move(RCI_ELBOW,30);
  melobot_move(RCI_GRIPPER,0);
}

static void handle_dunk_instruction (void)
{
  fprintf(stderr, "RCI Serv: Slam it home, Melobot!\n");

  melobot_move(RCI_WRIST,0);
  melobot_move(RCI_ELBOW,254);
  melobot_move(RCI_BASE,0);
  melobot_move(RCI_SHOULDER,254);
  melobot_move(RCI_SHOULDER,55);
  melobot_move(RCI_ELBOW,200);
  melobot_move(RCI_GRIPPER,254);
  melobot_move(RCI_ELBOW,254);
}

static int handle_rci_cmd (RCI_CMD *rci_cmd)
{
  int retval = 0;

  fprintf(stderr,
	  "RCI Serv: cmd = %d state = %d\n",
	  rci_cmd->cmd,
	  rci_state);
  
  switch (rci_cmd->cmd) {
  case CMD_KILL_REQUEST:
  {
    rci_stop_search_mode ();
    retval = -1;
  }
  break;

  case CMD_MOVE_REQUEST:
  {
    melobot_move (rci_cmd->u.move_request.joint, rci_cmd->u.move_request.pos);
    retval = 0;
  }
  break;

  case CMD_START_SEARCH_MODE:
  {
    rci_state = RCI_STATE_SEARCH;

    handle_search_instruction ();
    
    retval = 0;
  }
  break;

  case CMD_STOP_SEARCH_MODE:
  {
    stop_search = 1;
    retval = 0;
  }
  break;

  case CMD_GET_BALL_REQUEST:
  {
    rci_state = RCI_STATE_GRAB;

    handle_grab_instruction ();

    retval = 0;
  }
  break;

  case CMD_DUNK_REQUEST:
  {
    rci_state = RCI_STATE_DUNK;

    handle_dunk_instruction ();

    retval = 0;
  }
  break;

  default:
  break;
  }

  /* Return state to idle */
  rci_state = RCI_STATE_IDLE;

  return retval;
}

static void* comm_loop (void *parm)
{
  unsigned char buf[BUF_SZ];

  // Post on semaphore
  sem_post(&start_thread);

  fprintf(stderr, "RCI Serv: Start Listening on Socket\n");
  
  // Start listening to socket
  while (1)
  {
    int clientfd;
    int bytes_rcvd;
    struct sockaddr_in client_addr;
    int addrlen=sizeof(client_addr);

    // Zero out buf
    bzero(buf, BUF_SZ);
    
    // Accept a connection (creating a data pipe)
    clientfd = accept(sockserv,
                      (struct sockaddr*)&client_addr,
                      (socklen_t*)&addrlen);
    
    fprintf(stderr,
            "RCI Serv: %s:%d connected\n",
            inet_ntoa(client_addr.sin_addr),
            ntohs(client_addr.sin_port));

    // Read in the message from client
    bytes_rcvd = recv(clientfd, buf, BUF_SZ, 0);

    // Close data connection
    close(clientfd);

    // Handle the cmd
    if (handle_rci_cmd ((RCI_CMD*)buf))
    {
      fprintf(stderr, "RCI Serv: Breaking out of comm loop\n");
      break;
    }
  }

  // Clean up
  close(sockserv);

  return NULL;
}

static void init_comm_thread (void)
{
  pthread_attr_init(&comm_thread_attr);
  pthread_attr_setinheritsched(&comm_thread_attr, PTHREAD_EXPLICIT_SCHED);
  pthread_attr_setschedpolicy(&comm_thread_attr, SCHED_FIFO);

  rt_max_prio = sched_get_priority_max(SCHED_FIFO);
  rt_min_prio = sched_get_priority_min(SCHED_FIFO);

  // Make the comm thread a slightly lower prio (camview is rt_max_prio)
  comm_thread_sched_param.sched_priority = rt_max_prio - 1;
  pthread_attr_setschedparam(&comm_thread_attr, &comm_thread_sched_param);

  pthread_create(&comm_thread, &comm_thread_attr, comm_loop, (void *)NULL);

  // Wait for thread to start
  sem_wait (&start_thread);
}

static void rci_kill (void)
{
  RCI_CMD rci_cmd;

  fprintf(stderr, "RCI: KILL Request\n");

  // Send a packet to the comm_thread indicating that we are ready to terminate
  rci_cmd.cmd = CMD_KILL_REQUEST;
  send_buf_to_serv ((unsigned char*)&rci_cmd, sizeof(RCI_CMD));
}

void rci_init (void)
{
  // Open up the serial port for writing and reading
  fd = open(devicenm, O_RDWR | O_NOCTTY | O_NDELAY);
  if (fd == -1)
  {
    perror("open_port: Unable to open /dev/ttyS0 - ");
    exit(-1);
  }
  else
  {
    fcntl(fd, F_SETFL, 0);
  }

  // Initialize the 'start thread` semaphore
  sem_init(&start_thread, 0, 0);

  // Initialize the 'move' semaphore
  sem_init(&move_sem, 0, 0);

  // Get the current options for the port...
  tcgetattr(fd, &options);
  
  // Set the baud rates to 9600...
  cfsetispeed(&options, B9600);
  cfsetospeed(&options, B9600);

  // Enable the receiver and set local mode...
  options.c_lflag &= ~ICANON;
  options.c_cflag |= CLOCAL;

  // Robot serial interface is 8N1
  options.c_cflag &= ~PARENB;
  options.c_cflag &= ~CSTOPB;
  options.c_cflag &= ~CSIZE;
  options.c_cflag |= CS8;
  
  // Set the new options for the port...
  tcsetattr(fd, TCSANOW, &options);

  // Print out the baud rate
  fprintf(stderr,
          "RCI: Robot comm interface initialized at baud %d and 8N1\n",
          getbaud());

  // Initialize the server port
  init_comm_sockserv ();  

  // Spawn a thread to handle the robotics communication interface
  init_comm_thread ();

  // Set the initial position at pos 0 (run twice)
  set_initial_position ();
  set_initial_position ();

  fprintf(stderr,
	  "RCI: All servos set to position 0... "
	  "Melobot initialized\n");
}

void rci_uninit (void)
{
  fprintf(stderr, "RCI: Bye Melobot...\n");

  // Kill the comm thread
  pthread_kill (comm_thread, SIGALRM);

  // Close the socket
  close (fd);
}

void rci_start_search_mode (void)
{
  RCI_CMD rci_cmd;

  fprintf(stderr, "RCI: Start Search Mode Request\n");
  
  // Fill out cmd
  rci_cmd.cmd = CMD_START_SEARCH_MODE;

  // Send the command
  send_buf_to_serv ((unsigned char*)&rci_cmd, sizeof(RCI_CMD));
}

void rci_stop_search_mode (void)
{
  fprintf(stderr, "RCI: Stop Search Mode Request\n");

  stop_search = 1;
}

void rci_move (RCI_Joint_Specifier joint, unsigned char pos)
{
  RCI_CMD rci_cmd;

  fprintf(stderr, "RCI: Move Request (%d/%d)\n", joint, pos);
  
  // Fill out cmd
  rci_cmd.cmd = CMD_MOVE_REQUEST;
  rci_cmd.u.move_request.joint = joint;
  rci_cmd.u.move_request.pos = pos;
  
  // Send the command
  send_buf_to_serv ((unsigned char*)&rci_cmd, sizeof(RCI_CMD));

  // Sleep on semaphore waiting for command to complete
  sem_wait(&move_sem);
}

int rci_in_search_mode (void)
{
  return (rci_state == RCI_STATE_SEARCH);
}

void rci_grab (void)
{
  RCI_CMD rci_cmd;

  fprintf(stderr, "RCI: GRAB Request\n");
  
  // Fill out cmd
  rci_cmd.cmd = CMD_GET_BALL_REQUEST;

  // Send the command
  send_buf_to_serv ((unsigned char*)&rci_cmd, sizeof(RCI_CMD));
}

void rci_dunk (void)
{
  RCI_CMD rci_cmd;

  fprintf(stderr, "RCI: DUNK Request\n");
  
  // Fill out cmd
  rci_cmd.cmd = CMD_DUNK_REQUEST;

  // Send the command
  send_buf_to_serv ((unsigned char*)&rci_cmd, sizeof(RCI_CMD));
}
